#!/usr/bin/env python3
# -*- coding: utf-8 -*-

from __future__ import absolute_import
from __future__ import print_function
import rospy
from geometry_msgs.msg import Twist
# from std_msgs.msg import Float32
import sys, select, os
import tty, termios

BURGER_MAX_LIN_VEL = 0.18##设置BURGER速度参数
BURGER_MAX_ANG_VEL = 0.4

WAFFLE_MAX_LIN_VEL = 0.18##设置WAFFLE速度参数
WAFFLE_MAX_ANG_VEL = 0.4

LIN_VEL_STEP_SIZE = 0.02
ANG_VEL_STEP_SIZE = 0.1

FOMATION_CHANGE_SIZE = 0.05
MAX_FOMATION_CHANGE = 0.7

ANGLE_CHANGE_SIZE = 0.5#设置每次队形角度变化20220323
#MAX_CHANGE_SIZE = 4#设置最大角度变化，防止队形大于180度
MIN_CHANGE_SIZE = 2#设置最小角度变化，防止队形小于30度

msg = """
Control Your TurtleBot3!
---------------------------
Moving around:
        w
   a    s    d
        x

w/x : increase/decrease linear velocity (Burger : ~ 0.22, Waffle and Waffle Pi : ~ 0.26)
a/d : increase/decrease angular velocity (Burger : ~ 2.84, Waffle and Waffle Pi : ~ 1.82)

space key, s : force stop

CTRL-C to quit
"""

e = """
Communications Failed
"""

def getKey():
    # 把输入文件描述符模式改为raw,然后直接调用操作系的IO接口,监控所有带fileno()方法的文件句柄
    # tty.setraw():将文件描述符fd模式更改为raw
    # fileno():返回一个整形的文件描述符(fd)
    tty.setraw(sys.stdin.fileno())
     # select():直接调用操作系统的IO接口，它监控所有带fileon()方法的文件句柄
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1) # 只读取一个字节
    else:
        key = ''
    # termios该模块提供了一个用于tty I / O控制的POSIX调用的接口
    # tcsetattr:从属性设置文件描述符fd的tty属性
    # 参数二when: 参数确定属性何时发生更改：TCSANOW立即更改，TCSADRAIN在传输所有排队输出后更改，
    # 或TCSAFLUSH在传输所有排队输出并丢弃所有排队输入后更改
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

#20220322def vels(target_linear_vel, target_angular_vel, formation_size, autonomy):
#20220322    return "currently:\tlinear vel %s\t angular vel %s\t fomation changed %s\t autonomy %s\t " % (target_linear_vel,target_angular_vel,formation_size, autonomy)
def vels(target_linear_vel, target_angular_vel, formation_size, autonomy,formation_angl):
    return "currently:\tlinear vel %s\t angular vel %s\t fomation changed %s\t autonomy %s\t formation_angle %s\t" % (target_linear_vel,target_angular_vel,formation_size, autonomy, formation_angle)

def makeSimpleProfile(output, input, slop):
    if input > output:
        output = min( input, output + slop )
    elif input < output:
        output = max( input, output - slop )
    else:
        output = input

    return output

def constrain(input, low, high):  ##对输入量进行约束2022.3.18
    if input < low:
      input = low
    elif input > high:
      input = high
    else:
      input = input

    return input

def checkLinearLimitVelocity(vel):##设置小车速度在约束范围内
    if turtlebot3_model == "burger":
      vel = constrain(vel, 0.0 , BURGER_MAX_LIN_VEL)
    elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi":
      vel = constrain(vel, 0.0 , WAFFLE_MAX_LIN_VEL)
    else:
      vel = constrain(vel, 0.0 , BURGER_MAX_LIN_VEL)

    return vel

def checkFomationSize(vel):##设置小车队形缩小放大在约束范围内
    if vel < -MAX_FOMATION_CHANGE:
        vel = -MAX_FOMATION_CHANGE
    elif vel > MAX_FOMATION_CHANGE:
        vel = MAX_FOMATION_CHANGE
    else:
        vel = vel
    return vel

def checkAngleSize(vel):##设置小车队形最小角度由于角度公式实际上是不大于最大值20220323
    if vel < MIN_CHANGE_SIZE:
        vel = vel
    elif vel > MIN_CHANGE_SIZE:
        vel = MIN_CHANGE_SIZE
    else:
        vel = vel
    return vel

def checkAngularLimitVelocity(vel):
    if turtlebot3_model == "burger":
      vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL)
    elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi":
      vel = constrain(vel, -WAFFLE_MAX_ANG_VEL, WAFFLE_MAX_ANG_VEL)
    else:
      vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL)

    return vel

if __name__=="__main__":
    # tcgetattr():返回包含文件描述符fd的tty属性的列表
    settings = termios.tcgetattr(sys.stdin)

    rospy.init_node('cwm_interface')
    # pub = rospy.Publisher('/tb3_1/cmd_vel', Twist, queue_size=10)
    pub = rospy.Publisher('/cmd_user', Twist, queue_size=10)
    # pub = rospy.Publisher('tb3_0/cmd_vel', Twist, queue_size=10)

    turtlebot3_model = rospy.get_param("model", "waffle_pi")

    autonomy = -1
    autonomy_count = 0
    autonomy_exit = 0
    status = 0
    target_linear_vel   = 0.0
    target_angular_vel  = 0.0
    control_linear_vel  = 0.0
    control_angular_vel = 0.0
    formation_size = 0.0
    #队形角度变化 shn2022.3.20
    formation_angle =0
    angle_size = 0

    try:
        print(msg)
        while(1):
            key = getKey()
            if key == 'w' :   # 加速
                autonomy_count = 0
                autonomy_exit = 0
                formation_angle = formation_angle
                target_linear_vel = checkLinearLimitVelocity(target_linear_vel + LIN_VEL_STEP_SIZE)
                status = status + 1
                print((vels(target_linear_vel,target_angular_vel,formation_size, autonomy,formation_angle)))
            elif key == 'x' :  # 减速
                autonomy_count = 0
                autonomy_exit = 0
                formation_angle = formation_angle
                target_linear_vel = checkLinearLimitVelocity(target_linear_vel - LIN_VEL_STEP_SIZE)
                status = status + 1
                print((vels(target_linear_vel,target_angular_vel,formation_size, autonomy,formation_angle)))
            elif key == 'a' :  # 左转
                autonomy_count = 0
                autonomy_exit = 0
                formation_angle = formation_angle
                target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
                status = status + 1
                print((vels(target_linear_vel,target_angular_vel,formation_size, autonomy,formation_angle)))
            elif key == 'd' :  # 右转
                autonomy_count = 0
                autonomy_exit = 0
                formation_angle = formation_angle
                target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
                status = status + 1
                print((vels(target_linear_vel,target_angular_vel,formation_size, autonomy,formation_angle)))
            elif key == 'q' : # 缩小
                autonomy_count = 0
                autonomy_exit = 0
                formation_angle = formation_angle
                formation_size = checkFomationSize(formation_size - FOMATION_CHANGE_SIZE)

                print((vels(target_linear_vel,target_angular_vel,formation_size, autonomy,formation_angle)))
            elif key == 'z' : # 放大
                autonomy_count = 0
                autonomy_exit = 0
                formation_angle = formation_angle
                formation_size = checkFomationSize(formation_size + FOMATION_CHANGE_SIZE)
                print((vels(target_linear_vel,target_angular_vel,formation_size, autonomy,formation_angle)))
            elif key == 'f' : # 是否进入自动化命令
                autonomy_exit = 0
                autonomy_count = autonomy_count + 1
                print("autonomy_count: %d  autonomy: %d" %(autonomy_count, autonomy))
                if (autonomy_count >= 10):
                    autonomy_count = 0
                    autonomy = 1
                    print("!!!!!!autonomy_count: %d  autonomy: %d" %(autonomy_count, autonomy))
                print(vels(target_linear_vel,target_angular_vel,formation_size, autonomy))
               
            elif key == 'e' : # 队形变一字型 
                autonomy_exit = 0
                autonomy_count = autonomy_count + 1#累计次数20220323
                if (autonomy_count >= 5):#积累5次队形变成一字型
                    autonomy_count = 0
                    formation_angle = 1
                print((vels(target_linear_vel,target_angular_vel,formation_size, autonomy,formation_angle)))#20220322
            elif key == 'c' : # 队形变三角型
                autonomy_count = 0
                autonomy_exit = autonomy_exit + 1
                if (autonomy_exit >= 5):
                    autonomy_exit = 0
                    formation_angle = 0

                #formation_size = checkFomationSize(formation_size + FOMATION_CHANGE_SIZE)
                print((vels(target_linear_vel,target_angular_vel,formation_size, autonomy,formation_angle)))
            # elif key == 'e' : # 角度缩小（实际上是等腰三角形底角变大）
            #     autonomy_count = 0
            #     autonomy_exit = 0
            #     formation_angle = formation_angle#20220322
            #     angle_size = checkAngleSize(angle_size + ANGLE_CHANGE_SIZE)
            #     #20220322print(vels(target_linear_vel,target_angular_vel,formation_size, autonomy))
            #     print((vels(target_linear_vel,target_angular_vel,formation_size, autonomy,formation_angle)))#20220322
            # elif key == 'c' : # 角度扩大（实际上是等腰三角形底角变小）
            #     autonomy_count = 0
            #     autonomy_exit = 0
            #     formation_angle = formation_angle#20220322
            #     angle_size = angle_size - ANGLE_CHANGE_SIZE
            #     #20220322print(vels(target_linear_vel,target_angular_vel,formation_size, autonomy))
            #     print((vels(target_linear_vel,target_angular_vel,formation_size, autonomy,formation_angle)))#20220322




            # elif key == 'v' : ##转换队形为三角形2022.3.20
            #     autonomy_exit = 0
            #     autonomy_count = autonomy_count + 1
            #     # print("autonomy_count: %d  autonomy: %d" %(autonomy_count, autonomy))
            #     if (autonomy_count >= 10):
            #         autonomy_count = 0
            #         autonomy = 1
            #         formation_angle = 0#20220322
            #         # print("!!!!!!autonomy_count: %d  autonomy: %d" %(autonomy_count, autonomy))
            #     #20220322print(vels(target_linear_vel,target_angular_vel,formation_size, autonomy))
            #     print((vels(target_linear_vel,target_angular_vel,formation_size, autonomy,formation_angle)))#20220322
            elif key == ' ' or key == 's' :
                autonomy_count = 0
                autonomy_exit = autonomy_exit + 1
                if (autonomy_exit >= 10):
                    autonomy_exit = 0
                    autonomy = -1
                target_linear_vel   = 0.0
                control_linear_vel  = 0.0
                target_angular_vel  = 0.0
                control_angular_vel = 0.0
                formation_angle = formation_angle
                print((vels(target_linear_vel,target_angular_vel,formation_size, autonomy,formation_angle)))#20220322
                #20220322print(vels(target_linear_vel, target_angular_vel,formation_size, autonomy))
            else:
                if (key == '\x03'):
                    break

            if status == 20 :
                print(msg)
                status = 0

            twist = Twist()

            # control_linear_vel = makeSimpleProfile(control_linear_vel, target_linear_vel, (LIN_VEL_STEP_SIZE/2.0))
            control_linear_vel = target_linear_vel
            twist.linear.x = control_linear_vel; twist.linear.y = formation_size; twist.linear.z = autonomy

            # control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
            control_angular_vel = target_angular_vel
            #twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel
            #改变队形角度 shn 2022.3.20
            twist.angular.x = formation_angle; twist.angular.y = angle_size; twist.angular.z = control_angular_vel#引入y

            pub.publish(twist)

    except:
        print(e)

    finally:
        twist = Twist()
        twist.linear.x = 0.0; twist.linear.y = formation_size; twist.linear.z = -1
        #twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
        twist.angular.x = formation_angle; twist.angular.y = angle_size; twist.angular.z = 0.0
        pub.publish(twist)

    if os.name != 'nt':
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
